#include "DOFHandler.h"

#include <assert.h>

#include <algorithm>
#include <iostream>

#include "Utility/Logging/SGLogger.h"
#include "Utility/Configuration/SGConfig.h"

#include "DataStructure/Input/Constraint/RigidElementDef.h"
#include "DataStructure/Input/Element/ElementActiveDOFs.h"

#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Property/IPRODService.h"
#include "DBManager/IService/Input/Element/IElementService.h"

// Load
#include "DBManager/IService/Input/Load/ISPCDService.h"

// Constraint
#include "DBManager/IService/Input/Constraint/IMPCService.h"
#include "DBManager/IService/Input/Constraint/IRBAR1Service.h"
#include "DBManager/IService/Input/Constraint/IRBARService.h"
#include "DBManager/IService/Input/Constraint/IRBE2Service.h"
#include "DBManager/IService/Input/Constraint/IRBE3Service.h"
#include "DBManager/IService/Input/Constraint/IRJOINTService.h"
#include "DBManager/IService/Input/Constraint/IRRODService.h"
#include "DBManager/IService/Input/Constraint/IRSPLINEService.h"

// Connect element
#include "DBManager/IService/Input/Constraint/ISPCService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"

#include "SGFem/Pre/Common/PreInfo.h"
#include "SGFem/Pre/DOFHandler/MinimumDegreeSort.h"


using SG::DataStructure::Common::Id;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;

using namespace SG::Pre::Info;
using SG::FEMSolver::MinimumDegreeSort;

using namespace SG::FEMSolver;

// static  member
std::size_t                                DOFHandler::s_maxNodeDOFSize;
std::vector<SG::DataStructure::Common::Id> DOFHandler::s_nodeIds;    ///< 节点id
std::vector<std::size_t>                   DOFHandler::s_nodeindex;  ///< 节点在数据库中储存的索引

DOFHandler::DOFHandler (const std::vector<SG::DataStructure::Common::Id>& nodeIds,
                        SG::DataStructure::FEM::Field_Types               field,
                        const std::vector<std::size_t>&                   nodeindex)
    : m_field{ field }, m_nFreeDofs{ 0 }, m_nFixDofs{ 0 }, m_nSlaveDofs{ 0 }, m_dofGlobalIndex ()
{
    s_nodeIds        = nodeIds;
    s_nodeindex      = nodeindex;
    const auto nNode = s_nodeIds.size ();
    if (nNode == 0)
    {
        return;
    }

    if (s_nodeindex.empty ())
    {
        s_nodeindex.resize (nNode);
        for (size_t i = 0; i < s_nodeIds.size (); i++)
        {
            s_nodeindex[i] = i;
        }
    }

    // 需要对 nodeIds做升序排列，同时修改 indexes的顺序
    // n-1趟排序
    for (std::size_t i = 0; i < nNode - 1; i++)
    {
        // 一趟冒泡排序
        int flag = 0;
        for (std::size_t j = 0; j < nNode - i - 1; j++)
        {
            if (s_nodeIds[j] > s_nodeIds[j + 1])
            {
                auto tmp         = s_nodeIds[j];
                s_nodeIds[j]     = s_nodeIds[j + 1];
                s_nodeIds[j + 1] = tmp;

                auto tmp1          = s_nodeindex[j];
                s_nodeindex[j]     = s_nodeindex[j + 1];
                s_nodeindex[j + 1] = tmp1;

                flag = 1;
            }
        }

        // 若某一趟排序中没有元素交换则说明所有元素已经有序，不需要再排序
        if (flag == 0)
        {
            break;
        }
    }
}

void DOFHandler::MarkNodeActiveDOFs (std::size_t index, const DofSet& dofSet, _OUT std::vector<DOFTypes>& DOFFlag, std::size_t& nFreeDof)
{
    for (auto direction : dofSet)
    {
        assert (DOFDirections::DOF_Unknow != direction);
        auto posi = (index * s_maxNodeDOFSize) + direction;
        // 已经被激活自由度不再处理
        if (DOFTypes::DOFTYPE_Unknown == DOFFlag[posi])
        {
            ++nFreeDof;
            DOFFlag[posi] = DOFTypes::DOFTYPE_Free;
        }
    }
}

void DOFHandler::MarkNodeActiveDOFs (std::size_t index, DOFDirections dof, _OUT std::vector<DOFTypes>& DOFFlag, std::size_t& nFreeDof)
{
    assert (DOFDirections::DOF_Unknow != dof);
    auto posi = (index * s_maxNodeDOFSize) + static_cast<std::size_t> (dof);
    if (DOFTypes::DOFTYPE_Unknown == DOFFlag[posi])
    {
        ++nFreeDof;
        DOFFlag[posi] = DOFTypes::DOFTYPE_Free;
    }
}

/** @brief 遍历约束及多点约束方程，修改 DOFFlag相应位置的值
 */
void DOFHandler::MarkSPCSlaveDOFs (Id nodeId, const DofSet& dofSet, _OUT std::vector<DOFTypes>& DOFFlag)
{
    const auto nodeIndex = FindNodeIndex (nodeId);
    if (-1 == nodeIndex)
    {
        SGError << "Constraint not exist node " << nodeId;
        return;
    }

    const auto posi0 = nodeIndex * s_maxNodeDOFSize;
    for (const auto& dof : dofSet)
    {
        auto posi = posi0 + dof;
        if (DOFTypes::DOFTYPE_Free == DOFFlag[posi])
        {
            DOFFlag[posi] = DOFTypes::DOFTYPE_Constraint;
            --m_nFreeDofs;
            ++m_nFixDofs;
        }
        // 一个约束如果被约束两次， 应给出警告
        else if (DOFTypes::DOFTYPE_Constraint == DOFFlag[posi])
        {
            std::cout << nodeId << " '" << dof << " direction has been constrain more than once\n";
        }
        else if (DOFTypes::DOFTYPE_Unknown == DOFFlag[posi])
        {
            // 孤立点处理,spcd 位移恢复值和 spcd 设置结果相同
            DOFFlag[posi] = DOFTypes::DOFTYPE_Constraint;
            ++m_nFixDofs;
            // DOFFlag [posi] == DOFTYPE_Unknown 对应非激活的自由度， 不应该在该方向加约束.此时可以忽略错误
            std::cout << nodeId << " '" << dof << " direction is not active\n";
        }
        else
        {
            // DOFFlag [posi] == DOFTYPE_MPC, 被指定为从自由度的方向不应该被约束。程序应当立即报错
            std::cerr << "node " << nodeId << " '" << dof << " has error boundary condition" << std::endl;
            return;
        }
    }
}

void DOFHandler::MarkMPCDependentDOFs (std::size_t nodeId, const DofSet& dofSet, _OUT std::vector<DOFTypes>& DOFFlag, bool checkRepet)
{
    const auto nodeIndex = FindNodeIndex (nodeId);
    assert (nodeIndex >= 0);
    auto posi0 = nodeIndex * s_maxNodeDOFSize;
    // std::cout << "slave node " << sNode << " & its index  " << FindNodeIndex (sNode) << "\n";
    for (auto direction : dofSet)
    {
        assert (DOFDirections::DOF_Unknow != direction);
        auto posi = posi0 + static_cast<std::size_t> (direction);
        if (DOFTypes::DOFTYPE_Unknown == DOFFlag[posi])
        {
            // 仅仅被R单元使用， 用来施加载荷
            DOFFlag[posi] = DOFTypes::DOFTYPE_MPC;
            ++m_nSlaveDofs;
        }
        else if (DOFTypes::DOFTYPE_Free == DOFFlag[posi])
        {
            // 修改从自由度标记
            DOFFlag[posi] = DOFTypes::DOFTYPE_MPC;
            --m_nFreeDofs;
            ++m_nSlaveDofs;
        }
        else
        {
            static const std::string CONSTRAINERROR =
                std::to_string (nodeId) + " node's " + std::to_string (static_cast<std::size_t> (direction)) + "dof is constrained more than once !!!";
            // std::cout <<  << std::endl;
            // assert (0);
            if (checkRepet)
            {
                SGFatal << CONSTRAINERROR;
            }
        }
    }
}

/**
 * @brief    传热分析获取标记单元激活自由度
 * 
 * 对于CQUAD4， 结构分析每个节点具有 6个自由度，但是传热只有一个节点温度自由度，因此此函数和结构计算的对应函数分开
 */
void DOFHandler::ActiveThermalFieldDOF (const SG::DBManager::DBServiceFactorySP dbServiceFactory, _OUT std::size_t& nFreeDofs, _OUT std::vector<DOFTypes>& DOFFlag)
{
    const auto& EleSumInfoService = dbServiceFactory->get<IElementService> ();
    auto        eletypes          = EleSumInfoService->GetAllElementTypes ();
    for (const auto& type : eletypes)
    {
        auto       eleNodes    = EleSumInfoService->GetElementNodes (type);
        const auto eleSize     = eleNodes.GetRowSize ();
        const auto eleNodeSize = eleNodes.GetColSize ();
        for (std::size_t eleIndex = 0; eleIndex < eleSize; ++eleIndex)
        {
            for (std::size_t nodeJ = 0; nodeJ < eleNodeSize; ++nodeJ)
            {
                // 找到 0 无效节点
                auto nodeId = static_cast<Id> (eleNodes (eleIndex, nodeJ));
                // 单元节点数量不定， 返回节点Id 为 0；
                if (nodeId <= 0)
                {
                    continue;
                }
                const auto index = FindNodeIndex (nodeId);
                MarkNodeActiveDOFs (index, DOFDirections::DOF_Ux, DOFFlag, nFreeDofs);
            }
        }
    }
}

/**
 * @brief    遍历单元，将对应的节点自由度类型标记为DOFTYPE_Free， 并行更新独赢的自由度数量
 * 
 * 大多数单元类型激活自由度只需要获得有关的节点编号（各个节点的自由度通过单元类型确定； 调用 getElementActiveDofs 函数）
 * 
 * 例外： 1. ELETYPE_Link， ELETYPE_LinkNoRef 单元激活的自由度数量和 属性值中的截面积 A以及扭转刚度 J有关
 *        2. 大部分标量单元激活自由度数量需要查询单元中自由度方向
 * 
 * @param    dbServiceFactory 
 * @param    nFreeDofs 
 * @param    DOFFlag 
 * @author   wanghuan
 * @date     2025.07.20
 */
void DOFHandler::ActiveElasticFieldDOF (const SG::DBManager::DBServiceFactorySP dbServiceFactory, _OUT std::size_t& nFreeDofs, _OUT std::vector<DOFTypes>& DOFFlag)
{
    const auto& EleSumInfoService = dbServiceFactory->get<IElementService> ();
    // 遍历单元节点，激活所有自由度
    auto eletypes = EleSumInfoService->GetAllElementTypes ();
    for (const auto& type : eletypes)
    {
        // CWELD/CFAST 单元的 GA,GB 最终会进入 m-set, 所以此处不需要处理
        if (type == ElementTypes::ELETYPE_Weld || type == ElementTypes::ELETYPE_FAST)
        {
            continue;
        }

        // 杆单元激活自由度和其属性有关
        if (type == ElementTypes::ELETYPE_LinkNoRef)
        {
            CONRODData  rod;
            auto        nLink         = EleSumInfoService->Size<CONRODData> ();
            for (std::size_t eleIndex = 0; eleIndex < nLink; ++eleIndex)
            {
                EleSumInfoService->FindByIndex (eleIndex, rod);
                const DofSet dofSet = rod.GetActiveDofs ();
                auto   index  = FindNodeIndex (rod.m_g[0]);
                MarkNodeActiveDOFs (index, dofSet, DOFFlag, nFreeDofs);

                index = FindNodeIndex (rod.m_g[1]);
                MarkNodeActiveDOFs (index, dofSet, DOFFlag, nFreeDofs);
            }
            continue;
        }

        // 杆单元激活自由度和其属性有关
        if (type == ElementTypes::ELETYPE_Link)
        {
            CRODData                         rod;
            SG::DataStructure::FEM::PRODData prod;
            auto                             nLink = EleSumInfoService->Size<CRODData> ();
            for (std::size_t eleIndex = 0; eleIndex < nLink; ++eleIndex)
            {
                EleSumInfoService->FindByIndex (eleIndex, rod);
                const auto pid = rod.m_pId;
                // 获取截面属性
                dbServiceFactory->get<IPRODService> ()->FindById (pid, prod);
                const DofSet dofSet = prod.GetActiveDofs ();

                auto index = FindNodeIndex (rod.m_g[0]);
                MarkNodeActiveDOFs (index, dofSet, DOFFlag, nFreeDofs);

                index = FindNodeIndex (rod.m_g[1]);
                MarkNodeActiveDOFs (index, dofSet, DOFFlag, nFreeDofs);
            }
            continue;
        }

        if (ElementTypes::ELETYPE_ScalarSpring == type)
        {
            CELAS1Data spring;
            auto nScalarEle = EleSumInfoService->GetElementSize (type);
            for (std::size_t iEle = 0; iEle < nScalarEle; ++iEle)
            {
                EleSumInfoService->FindByIndex (iEle, spring);
                auto index  = FindNodeIndex (spring.m_g[0]);
                auto dofSet = spring.GetDOF1 ();
                MarkNodeActiveDOFs (index, { dofSet }, _OUT DOFFlag, nFreeDofs);

                index  = FindNodeIndex (spring.m_g[1]);
                if (index > 0)
                {
                dofSet = spring.GetDOF2 ();
                MarkNodeActiveDOFs (index, { dofSet }, _OUT DOFFlag, nFreeDofs);
                }
            }
            continue;
        }

        if (ElementTypes::ELETYPE_ScalarSpring2 == type)
        {
            CELAS2Data spring;
            auto nScalarEle = EleSumInfoService->GetElementSize (type);
            for (std::size_t iEle = 0; iEle < nScalarEle; ++iEle)
            {
                EleSumInfoService->FindByIndex (iEle, spring);
                auto index  = FindNodeIndex (spring.m_g[0]);
                auto dofSet = spring.GetDOF1 ();
                MarkNodeActiveDOFs (index, { dofSet }, _OUT DOFFlag, nFreeDofs);

                index  = FindNodeIndex (spring.m_g[1]);
                if (index > 0)
                {
                dofSet = spring.GetDOF2 ();
                MarkNodeActiveDOFs (index, { dofSet }, _OUT DOFFlag, nFreeDofs);
                }
            }
            continue;
        }

        // 其他单元 (单元的节点具有相同的自由度方向)
        const DofSet     dofSet      = getElementActiveDofs (type);
        auto       eleNodes    = EleSumInfoService->GetElementNodes (type);
        const auto eleSize     = eleNodes.GetRowSize ();
        const auto eleNodeSize = eleNodes.GetColSize ();
        for (std::size_t eleIndex = 0; eleIndex < eleSize; ++eleIndex)
        {
            for (std::size_t nodeJ = 0; nodeJ < eleNodeSize; ++nodeJ)
            {
                const auto index = FindNodeIndex (static_cast<Id> (eleNodes (eleIndex, nodeJ)));
                MarkNodeActiveDOFs (index, dofSet, DOFFlag, nFreeDofs);
            }
        }
    }

    // R单元激活主自由度
    {
    const auto nRjoint = dbServiceFactory->get<IRJOINTService> ()->Size ();
    RJOINTData rjoint;
    for (std::size_t iRJoint = 0; iRJoint < nRjoint; ++iRJoint)
    {
        dbServiceFactory->get<IRJOINTService> ()->FindByIndex (iRJoint, _OUT rjoint);
        const auto index  = FindNodeIndex (rjoint.m_masterNode);
        const DofSet     dofSet = rjoint.GetMasterDOFs ();
        MarkNodeActiveDOFs (index, dofSet, _OUT DOFFlag, nFreeDofs);  // 主自由度和从自由度相同
    }

    // RBAR1单元激活主节点的自由度
    const auto nRbar1 = dbServiceFactory->get<IRBAR1Service> ()->Size ();
    RBAR1Data  rbar1;
    for (std::size_t iBAR1 = 0; iBAR1 < nRbar1; ++iBAR1)
    {
        dbServiceFactory->get<IRBAR1Service> ()->FindByIndex (iBAR1, _OUT rbar1);
        const auto   index  = FindNodeIndex (rbar1.m_masterNode);
        const DofSet dofSet = SG::DataStructure::FEM::getAllNodeDofs ();
        MarkNodeActiveDOFs (index, dofSet, _OUT DOFFlag, nFreeDofs);
    }

    // RBE2单元激活主节点的自由度
    const auto nRbe2 = dbServiceFactory->get<IRBE2Service> ()->Size ();
    RBE2Data   rbe2;
    for (std::size_t iRBE2 = 0; iRBE2 < nRbe2; ++iRBE2)
    {
        dbServiceFactory->get<IRBE2Service> ()->FindByIndex (iRBE2, _OUT rbe2);
        const auto index  = FindNodeIndex (rbe2.m_masterNode);
        const DofSet     dofSet = SG::DataStructure::FEM::getAllNodeDofs ();
        MarkNodeActiveDOFs (index, dofSet, DOFFlag, nFreeDofs);
    }

    // RBAR单元激活主节点的自由度
    const auto nRbar = dbServiceFactory->get<IRBARService> ()->Size ();
    RBARData   rbar;
    for (std::size_t iRBAR = 0; iRBAR < nRbar; ++iRBAR)
    {
        dbServiceFactory->get<IRBARService> ()->FindByIndex (iRBAR, _OUT rbar);
        const DofSet dofSet1 = rbar.GetMasterDof1 ();
        const auto   index1  = FindNodeIndex (rbar.m_node1);
        MarkNodeActiveDOFs (index1, dofSet1, DOFFlag, nFreeDofs);

        const DofSet dofSet2 = rbar.GetMasterDof2 ();
        const auto   index2  = FindNodeIndex (rbar.m_node2);
        MarkNodeActiveDOFs (index2, dofSet2, DOFFlag, nFreeDofs);
    }

    // RBE3单元激活主节点的自由度
    const auto nRbe3 = dbServiceFactory->get<IRBE3Service> ()->Size ();
    RBE3Data   rbe3;
    for (std::size_t iRBE3 = 0; iRBE3 < nRbe3; ++iRBE3)
    {
        dbServiceFactory->get<IRBE3Service> ()->FindByIndex (iRBE3, _OUT rbe3);
        // DofSet     dofSet = SG::DataStructure::FEM::getAllNodeDofs ();  // ToDo:是不是不对？_hjx
        // const auto index  = FindNodeIndex (rbe3.m_refg);
        // MarkNodeActiveDOFs (index, dofSet, DOFFlag, nFreeDofs);
        // for (std::size_t iGroup = 0; iGroup < rbe3.m_WTCG.size (); iGroup++)
        // {
        //     for (const auto& mNode : rbe3.m_WTCG[iGroup].m_g)
        //     {
        //         const auto index = FindNodeIndex (mNode);
        //         MarkNodeActiveDOFs (index, dofSet, DOFFlag, nFreeDofs);
        //     }
        // }
    }

    // RSpline单元激活主节点的自由度
    const auto  nRspline = dbServiceFactory->get<IRSPLINEService> ()->Size ();
    RSPLINEData rspline;
    for (std::size_t iRspline = 0; iRspline < nRspline; ++iRspline)
    {
        dbServiceFactory->get<IRSPLINEService> ()->FindByIndex (iRspline, rspline);
        for (const auto& item : rspline.m_items)
        {
            if (item.GetDofs () != SG::DataStructure::FEM::getAllNodeDofs ())
            {
                const auto nodeId = item.m_nodeId;
                const auto index  = FindNodeIndex (nodeId);
                const DofSet     dofSet = item.GetDofs ();
                MarkNodeActiveDOFs (index, dofSet, DOFFlag, nFreeDofs);
            }
        }
    }
    }  // end R单元激活主自由度
}

/** @brief 标记节点自由度对应的类型
 *
 * 假设每个节点最大具有 `s_maxNodeDOFSize`个自由度(`s_maxNodeDOFSize`与求解问题域的类型有关)，
 * 初始化一个长度为 [s_maxNodeDOFSize*网格中节点数量]的vector, 此时索引自由度全部未必激活</p>
 * 遍历单元，依次将单元节点的自由度标记修改为 \ref DOFTypes::DOFTYPE_Free。 成员变量`m_nFreeDofs`记录自由自由度的总数 </p>
 * 遍历各种约束，将自由自由度标记修改成指定的自由度类型（目前仅仅支持 固定约束 DOFTypes::DOFTYPE_Constrain）。
 * 由于自由度类型发生改变，对应的自由度数量`m_nFreeDofs`数量减一，而相应的约束自由度数量加一
 */
void DOFHandler::MarkDOFType (const SG::DBManager::DBServiceFactorySP dbServiceFactory, const Subcase& subcase, std::vector<DOFTypes>& _OUT DOFFlag)
{
    typedef void (*FuncPtr) (
        const SG::DBManager::DBServiceFactorySP, _OUT std::size_t&, _OUT std::vector<SG::DataStructure::FEM::DOFTypes>&);
    FuncPtr func;

    switch (m_field)
    {
    case Field_Types::FIELD_Elastic:
        s_maxNodeDOFSize = 6;
        func             = DOFHandler::ActiveElasticFieldDOF;
        break;
    case Field_Types::FIELD_Thermal:
        s_maxNodeDOFSize = 1;
        func             = DOFHandler::ActiveThermalFieldDOF;
        break;

    default:
        std::cerr << "Cant support field \n";
        break;
    }

    // 将所有节点的自由度都初始化未 未知类型
    auto nodes = s_nodeIds.size ();
    DOFFlag    = std::vector<DOFTypes> (nodes * s_maxNodeDOFSize, DOFTypes::DOFTYPE_Unknown);

    assert (nullptr != func);
    func (dbServiceFactory, m_nFreeDofs, _OUT DOFFlag);


    // MPC激活主节点的自由度
    {
        std::vector<MPCData> mpcs;
        dbServiceFactory->get<IMPCService> ()->FindBySId (subcase.GetMPCId (), _OUT mpcs);
        for (const auto& mpc : mpcs)
        {
            for (const auto& item : mpc.m_masterItems)
            {
                const auto index = FindNodeIndex (item.m_nodeId);
                MarkNodeActiveDOFs (index, item.GetDofs (), DOFFlag, m_nFreeDofs);
            }
        }  // end MPC
    }

    // 标记节点单点约束自由度 (对应spc 和 spc1 数据)
    SG::DataStructure::FEM::SPCDataArray spcSet;
    dbServiceFactory->get<ISPCService> ()->FindBySId (subcase.GetSPCId(), _OUT spcSet);
    for (const auto& spc : spcSet)
    {
        MarkSPCSlaveDOFs (spc.m_g, spc.GetIndexes (), _OUT DOFFlag);
    }

    // 标记指定位移`spcd`标记的自由度
    SG::DataStructure::FEM::SPCDataArray spcdSet;
    dbServiceFactory->get<ISPCDService> ()->FindBySId (subcase.GetLoadId(), spcdSet);
    for (const auto& spc : spcdSet)
    {
        MarkSPCSlaveDOFs (spc.m_g, spc.GetIndexes (), _OUT DOFFlag);
    }

    // Grid PS
    auto     nodeRepository = dbServiceFactory->get<INodeService> ();
    NodeData node;
    auto     nNode = nodeRepository->Size ();
    for (std::size_t iNode = 0; iNode < nNode; ++iNode)
    {
        nodeRepository->FindByIndex (iNode, _OUT node);
        const auto ps = node.GetConstraint ();
        if (!ps.empty ())
        {
            MarkSPCSlaveDOFs (node.m_id, ps, _OUT DOFFlag);
        }
    }

    // R单元对应从自由度标记为，同时更新约束和自由的【自由度集合】大小
    // 将RROD单元对应从自由度标记为相应类型
    const auto rrodRepo = dbServiceFactory->get<IRRODService> ();
    const auto nRrod = rrodRepo->Size ();
    RRODData   rrod;
    for (std::size_t index = 0; index < nRrod; ++index)
    {
        rrodRepo -> FindByIndex (index, _OUT rrod);
        auto dofSet1 = rrod.GetDOF1();
        if (DOFDirections::DOF_Unknow == dofSet1)
        {
            auto dofSet2 = rrod.GetDOF2();
            MarkMPCDependentDOFs (rrod.m_G2, {dofSet2},  _OUT DOFFlag);
        }
        else
        {
            
            MarkMPCDependentDOFs (rrod.m_G1, {dofSet1},  _OUT DOFFlag);
        }
    }

    // 将RJoint单元对应从自由度标记为相应类型
    const auto nRjoint = dbServiceFactory->get<IRJOINTService> ()->Size ();
    RJOINTData rjoint;
    for (std::size_t index = 0; index < nRjoint; ++index)
    {
        dbServiceFactory->get<IRJOINTService> ()->FindByIndex (index, _OUT rjoint);
        const DofSet dofSet = rjoint.GetMasterDOFs ();
        MarkMPCDependentDOFs (rjoint.m_slaveNode, dofSet, _OUT DOFFlag);
    }

    // 将RBAR1单元对应从自由度标记为DOFTYPE_MPC
    const auto nRbar1 = dbServiceFactory->get<IRBAR1Service> ()->Size ();
    RBAR1Data  rbar1;
    for (std::size_t index = 0; index < nRbar1; ++index)
    {
        dbServiceFactory->get<IRBAR1Service> ()->FindByIndex (index, _OUT rbar1);
        const DofSet dofSet = rbar1.GetSlaveDOFs ();
        MarkMPCDependentDOFs (rbar1.m_slaveNodes, dofSet, _OUT DOFFlag);
    }

    // 将RBE2单元对应从自由度标记为相应类型，更改同时更新数量
    RBE2Data    rbe2;
    const auto& rbe2Repository = dbServiceFactory->get<IRBE2Service> ();
    const auto  nRbe2          = rbe2Repository->Size ();
    for (std::size_t index = 0; index < nRbe2; ++index)
    {
        rbe2Repository->FindByIndex (index, _OUT rbe2);
        // 遍历从节点
        const DofSet dofSet = rbe2.GetSlaveDofs ();
        for (const auto& sNode : rbe2.m_slaveNodes)
        {
            MarkMPCDependentDOFs (sNode, dofSet, _OUT DOFFlag);
        }
    }

    // 将RBAR单元对应从自由度标记为相应类型
    RBARData   rbar;
    const auto nRbar = dbServiceFactory->get<IRBARService> ()->Size ();
    for (std::size_t index = 0; index < nRbar; ++index)
    {
        dbServiceFactory->get<IRBARService> ()->FindByIndex (index, _OUT rbar);
        const DofSet dofSet1 = rbar.GetSlaveDof1 ();
        MarkMPCDependentDOFs (rbar.m_node1, dofSet1, DOFFlag);
        const DofSet dofSet2 = rbar.GetSlaveDof2 ();
        MarkMPCDependentDOFs (rbar.m_node2, dofSet2, DOFFlag);
    }

    // 将RBE3单元对应从自由度标记为相应类型
    const auto nRbe3 = dbServiceFactory->get<IRBE3Service> ()->Size ();
    RBE3Data   rbe3;
    for (std::size_t index = 0; index < nRbe3; ++index)
    {
        dbServiceFactory->get<IRBE3Service> ()->FindByIndex (index, _OUT rbe3);
        MarkMPCDependentDOFs (rbe3.m_refg, rbe3.GetREFDOF (), _OUT DOFFlag);
    }

    // RSpline单元激活从节点的自由度
    const auto  nRspline = dbServiceFactory->get<IRSPLINEService> ()->Size ();
    RSPLINEData rspline;
    for (std::size_t iRspline = 0; iRspline < nRspline; ++iRspline)
    {
        dbServiceFactory->get<IRSPLINEService> ()->FindByIndex (iRspline, rspline);
        for (const auto& item : rspline.m_items)
        {
            if (!item.GetDofs ().empty ())
            {
                const auto nodeId = item.m_nodeId;
                const DofSet     dofSet = item.GetDofs ();
                MarkMPCDependentDOFs (nodeId, dofSet, _OUT DOFFlag);
            }
        }
    }

    // 将MPC对应从自由度标记为相应类型
    std::vector<MPCData> mpcs;
    dbServiceFactory->get<IMPCService> ()->FindBySId (subcase.GetMPCId (), _OUT mpcs);
    for (const auto& mpc : mpcs)
    {
        const auto   nodeId = mpc.m_slaveItem.m_nodeId;
        const DofSet dofSet = mpc.m_slaveItem.GetDofs ();
        MarkMPCDependentDOFs (nodeId, dofSet, DOFFlag);
    }

    static const DofSet alldofSet = SG::DataStructure::FEM::getAllNodeDofs ();
    static const DofSet translateDofSet{DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz};

    // mark m-set, caused by CWELD element 
    const auto eleRepo = dbServiceFactory->get<IElementService> ();
    const auto nCweld = eleRepo -> Size<CWELDData>();
    CWELDData cweld;
    for (std::size_t index = 0; index < nCweld; ++index)
    {
        eleRepo -> FindByIndex (index, _OUT cweld);
        MarkMPCDependentDOFs (cweld.m_g[0], alldofSet, DOFFlag, true);
        MarkMPCDependentDOFs (cweld.m_g[1], alldofSet, DOFFlag, true);
    }

    // mark m-set, caused by CFAST element
    // 商飞客户要求， CFAST不使用辅助点
    bool customized{false};
    const auto& config = SG::Utility::SGConfig::GetInstance ().GetData ();
    if (config.Has("CFAST"))
    {
        auto FASTSetting = config.GetValue("CFAST");
        customized = FASTSetting.GetValue("SFCustomized").GetBool();
    }

    const auto& allCFast = eleRepo ->GetAllData<CFASTData>();
    for (const auto& cFast : allCFast)
    {
        MarkMPCDependentDOFs (cFast.m_g[0], alldofSet, DOFFlag, true);
        MarkMPCDependentDOFs (cFast.m_g[1], alldofSet, DOFFlag, true);
        if (!customized)
        {
            for (std::size_t auxIndex{0}; auxIndex < 8; ++auxIndex)
            {
                MarkMPCDependentDOFs (cFast.m_gh[auxIndex], translateDofSet, DOFFlag);
            }
        }
    }

    SGDebug << "@  nFreeDofs = " << m_nFreeDofs;
    SGDebug << "@  nSPCDofs = " << m_nFixDofs;
    SGDebug << "@  nMPCDofs = " << m_nSlaveDofs;
}

/** @brief 节点全局自由度排序
 *
 *  依次遍历每个自由度，按照顺序给自由度指定一个全局编号（存储在成员变量 m_dofGlobalIndex中）</p>
 *      独立自由度
 *      spc约束自由度
 *      多点约束方程从自由度
 *  约束处理先对多点约束进行处理， 在处理 SPC
 */
void DOFHandler::Sort (const std::vector<DOFTypes>& DOFFlag)
{
    /// 自由度按照 {自由 约束 }的顺序重新排序
    auto nDof        = DOFFlag.size ();
    m_dofGlobalIndex = std::vector<int> (nDof, -1);

    /** 遇到下一个某种类型自由度， 自由度应该被指定的全局编号
     *  posi[0] DOFTYPE_Free类型自由度的编号， 第一个本类型的自由度编号为0。
     *          posi[0]被赋值给遇到的DOFTYPE_Free类型自由度后：其数值+1，供下一个DOFTYPE_Free类型自由度使用
     *  posi[1] 第一个固定约束以及强制位移的约束自由度的编号， 初始值为为m_nFreeDofs;
     *  posi[2] MPC（多点约束方程）产生的消去自由度， 其初始编号为m_nFreeDofs + ，
     *          为遇到DOFTYPE_MPC类型自由度的正确编号。
     *          编号完毕，其数值加1，供下一个DOFTYPE_MPC类型自由度使用
     *
     */
    int posi[3];  // 需要每次增加自由度类型时，进行修改。
    posi[0] = 0;
    posi[1] = static_cast<int>(m_nFreeDofs);
    posi[2] = static_cast<int>(posi[1] + m_nFixDofs);

    for (std::size_t index = 0; index < nDof; ++index)
    {
        switch (DOFFlag[index])
        {
        case DOFTypes::DOFTYPE_Free: {
            m_dofGlobalIndex[index] = posi[0];
            posi[0] += 1;
            break;
        }
        case DOFTypes::DOFTYPE_Constraint: {
            m_dofGlobalIndex[index] = posi[1];
            posi[1] += 1;
            break;
        }
        case DOFTypes::DOFTYPE_MPC: {
            m_dofGlobalIndex[index] = posi[2];
            posi[2] += 1;
            break;
        }
        default:
            break;  // do nothing
        }
    }
    // For Test
    // if(1)
    // {
    //     auto nodeSize = m_dofGlobalIndex.size () / 6;
    //     std::size_t index = 0;
    //     for (std::size_t iNode = 0; iNode < nodeSize; ++iNode)
    //     {
    //         for (std::size_t iDof = 0; iDof < 6; ++iDof)
    //         {
    //             std::cout << m_dofGlobalIndex[index] << " ";
    //             ++index;
    //         }
    //         std::cout << "\n";
    //     }
    // }

    // ToDo 增加校验？ 运行完毕后，  posi数组中向量的正确值？
}

void DOFHandler::Compute (const DBServiceFactorySP dbServiceFactory, const Subcase& subcase)
{
    // auto g = MinimumDegreeSort::GenerateGraph (dbServiceFactory, s_nodeIds, s_nodeindex);
    std::vector<DOFTypes> DOFFlag;
    MarkDOFType (dbServiceFactory, subcase, DOFFlag);
    Sort (DOFFlag);
}

/** @brief 通过Id 查询节点的索引
 *
 * m_nodeIds已经完成了排序，使用二分查找确定索引的id
 */
int DOFHandler::FindNodeIndex (SG::DataStructure::Common::Id nodeId)
{
    int  left  = 0;
    int  right = static_cast<int>(s_nodeIds.size ()) - 1;
    auto posi  = 0;
    while (left <= right)
    {
        const int middle = left + ((right - left) / 2);
        if (s_nodeIds[middle] > nodeId)
        {
            right = middle - 1;
        }
        else if (s_nodeIds[middle] < nodeId)
        {
            left = middle + 1;
        }
        else
        {
            posi = middle;
            if (s_nodeIds[posi] != nodeId)
            {
                std::cerr << Not_Find_Node_With_That_Id;
                return -1;
            }
            return s_nodeindex[posi];
        }
    }

    SGError << "Can't find node with id " << nodeId;
    return -1;
}

int DOFHandler::GetGlobalIndex (const std::size_t nodeIndex, const SG::DataStructure::FEM::DOFDirections& dof) const
{
    auto position = (nodeIndex * s_maxNodeDOFSize) + static_cast<std::size_t> (dof);
    return m_dofGlobalIndex[position];
}

/** @brief 通过一组节点的index以及对应的激活自由度， 查询自由度对应的全局索引编号
 *
 */
void DOFHandler::GetGlobalIndex (const std::vector<std::size_t>& nodeIndexes,
                                 const DofSet&                   DOFs,
                                 _OUT std::vector<int>& dofGlobalIndex) const
{
    const auto nDof  = DOFs.size ();
    const auto nNode = nodeIndexes.size ();
    dofGlobalIndex.resize (nDof * nNode);

    std::size_t index{ 0 };
    for (const auto& nodeIndex : nodeIndexes)
    {
        auto position = nodeIndex * s_maxNodeDOFSize;
        for (const auto& dof : DOFs)
        {
            dofGlobalIndex[index] = m_dofGlobalIndex[position + dof];
            ++index;
        }
    }
}